#include "demo1.h"

#if IMG_WRITE
char img_path[100]      = "/home/nano/Desktop/src/img/img_%d.jpg";
char img_blue_path[100] = "/home/nano/Desktop/src/img_blue/img_blue_%d.jpg";
char img_k_path[100]    = "/home/nano/Desktop/src/img_k/img_k_%d.jpg";
#endif

pair<int, int> TraceLine::calculate_short_blue(Mat& img)
{
    Mat img_hsv, mask_gray;
    Mat kernel, img_erode;
    cvtColor(img, img_hsv, COLOR_BGR2HSV);
	inRange(img_hsv, Scalar(112, 30, 50), Scalar(159, 255,220), mask_gray);
   // inRange(img_hsv, Scalar(112, 53, 62), Scalar(159, 229, 192), mask_gray);
    kernel = getStructuringElement(MORPH_RECT, Size(3, 3), Point(0, 0));

    int s = 0, w = 0, h = 0;
    for ( int i = 270; i < 480; i++ ) {
        for ( int j = 0; j < 400; j++ ) {
            if ( mask_gray.at<uchar>(i, j) == 255 ) {
                s++;
                w += j;
                h += i;
            }
        }
    }

    h /= s;
    w /= s;

    pair<int, int> p;
    if ( s < 10 ) {
        p.first  = 0;
        p.second = 0;
    }
    else {
        p.first  = w;
        p.second = h;
        cout << "(" << p.first << "," << p.second << ")" << endl;
    }

#if IMG_SHOW
    imshow("erode", mask_gray);
#endif
    return p;
}
TraceLine::TraceLine(const vector<int>& q1)
{
    left_sum = 0;
    cnt      = 0;
    for ( auto& i : q1 ) {
        q.push(i);
    }
    read_src();
    left_sign = 0;
}

Line TraceLine::calculate_blue(Mat& img)
{
    // 先提取蓝色，再进行霍夫直线变换，存储在lines
    Mat           img_hsv;
    Mat           mask_gray;
    Mat           img_edges;
    Mat           img_erode, kernel;
    vector<Vec4f> ls;
    cvtColor(img, img_hsv, COLOR_BGR2HSV);
    inRange(img_hsv, Scalar(96, 25, 63), Scalar(159, 229, 255), mask_gray);
    kernel = getStructuringElement(MORPH_RECT, Size(3, 3), Point(0, 0));
    erode(mask_gray, img_erode, kernel);
    Canny(img_erode, img_edges, 200, 300);

    HoughLinesP(img_edges, ls, 1, (double)3.1416 / 180, 50, 200, 70);

    // lines为空的特殊处理
    if ( ls.size() == 0 ) {
        return {};
    }

    // 斜率
    vector<Line> lines;
    for ( auto& l : ls ) {
        double k = (l[1] - l[3]) / (l[0] - l[2]);
        if ( abs(k) < 1 ) {
            lines.emplace_back(k, l[0], l[1], l[2], l[3]);
        }
    }
    if ( lines.empty() ) {
        return {};
    }

    // 运算符重载，按照长度从大到小排序
    sort(lines.begin(), lines.end());
    Line t = lines[0];

#if IMG_SHOW
    line(img, Point(t.x1, t.y1), Point(t.x2, t.y2), Scalar(0, 0, 255), 2);
    imshow("blue", img);
#endif
#if IMG_WRITE
    line(img, Point(t.x1, t.y1), Point(t.x2, t.y2), Scalar(0, 0, 255), 2);
    sprintf(img_blue_path, "/home/nano/Desktop/src/img_blue/img_blue_%d.jpg", cnt);
    imwrite(img_blue_path, img);
#endif

    // 返回了最长的Line
    return lines[0];
}

pair<int, int> TraceLine::calculate_k(Mat img)
{
    // 霍夫直线变换，存储在lines
    Mat gray, img_erode, edges, hsv_img, kernel, white;
    cvtColor(img, hsv_img, COLOR_BGR2GRAY);
    kernel = getStructuringElement(MORPH_RECT, Size(3, 3), Point(0, 0));
    erode(hsv_img, img_erode, kernel);
    Canny(img_erode, edges, 200, 255);
    vector<Vec4f> ls;
    HoughLinesP(edges, ls, 1, 3.1415926535 / 180, 50, 50, 30);
    // 如果为空，直接返回
    if ( ls.size() == 0 ) {
        return make_pair(-INF, INF);
    }

    // 在斜率范围内的直线进行记录
    vector<Line> zheng;
    vector<Line> fu;
    for ( auto& l : ls ) {
        if ( l[0] == l[2] )
            continue;
        double k = (l[1] - l[3]) / (l[0] - l[2]);
        Line   lt(k, l[0], l[1], l[2], l[3]);
        if ( k > 0.3 && k < 1.3 && lt.x1 > 200 )
            zheng.push_back(lt);
        else if ( k < -0.3 && k > -0.8 && lt.x1 < 400 )
            fu.push_back(lt);
    }

    // 两侧方向都找最长的
    pair<int, int> lr;
    if ( !zheng.empty() ) {
        sort(zheng.begin(), zheng.end(), Line::cmp1);
        Line t1   = zheng[0];
        lr.second = t1.get_dots();
        line(img, Point(t1.x1, t1.y1), Point(t1.x2, t1.y2), Scalar(0, 0, 255), 2);
    }
    else {
        lr.second = INF;
    }

    if ( !fu.empty() ) {
        sort(fu.begin(), fu.end());
        Line t1  = fu[0];
        lr.first = t1.get_dots();
        line(img, Point(t1.x1, t1.y1), Point(t1.x2, t1.y2), Scalar(0, 255, 0), 2);
    }
    else {
        lr.first = -INF;
    }
#if IMG_SHOW
    imshow("lines", img);
    waitKey(5);
#endif
#if IMG_WRITE
    sprintf(img_k_path, "/home/nano/Desktop/src/img_k/img_k_%d.jpg", cnt);
    imwrite(img_k_path, img);
#endif
    return lr;
}

vector<int> TraceLine::calculate_predict()
{
    // N中存储了五个档位下重合像素点的数量
    vector<int> N = {0, 0, 0, 0, 0};

    Mat filtered_image;
    Mat gray;
    cvtColor(img_resize, gray, COLOR_BGR2GRAY);

    Mat kernel =
        getGaborKernel(Size_<int>(20, 20), 6.5, 2.83, 13.2, 5, 0, CV_32F);
    filter2D(gray, filtered_image, CV_8UC3, kernel);

    Mat thresh;
    threshold(filtered_image, thresh, 230, 255, THRESH_TOZERO);

    // 古老的遍历计算数量
    for ( int j = 0; j < 5; j++ ) {
        for ( int x = 340; x < 477; x++ ) {
            int low  = T[j][x - 340].first;
            int high = T[j][x - 340].second;
            for ( int y = low; y < high; y++ ) {
                if ( thresh.at<uchar>(x, y) == 255 ) {
                    N[j]++;
                }
            }
        }
    }
    return N;
}

char TraceLine::strategy_blue(Line& line)
{
    ros::param::get("stim", stim);
    if ( stim == 2 and state == DAOCHEYOUZHAUAN )
        return '3';
    if ( stim == 0 and state == DAOCHEYOUZHAUAN )
        return '5';
    if ( stim == N_CHANGE_STIM && state == RIGHT )
        return '5';
    if ( stim == N_CHANGE_STIM && state == LEFT ) {
        return '0';
    }

    if ( line.k == -INF ) {
        return '0';
    }

    // 长度大于150，并且允许更改stim的话
    if ( line.length > 300 && stim == Y_CHANGE_STIM ) {
        line.length = 0;
        q.pop();
        state = q.front();
        ros::param::set("state", state);
        ros::param::get("stim", stim);
        if ( stim == 1 ) {
            ros::param::set("stim", 0);
        }
        cout << "stim is set to be " << 0 << endl;
        cout << "see blue line ,length is " << line.length << endl;
    }
    if ( line.k < -0.1 )
        return '1';
    else if ( line.k < 0.02 )
        return '2';
    else if ( line.k < 0.05 )   // 0.04
        return '3';
    else if ( line.k < 0.2 )
        return '4';
    else if ( line.k < 0.3 )
        return '5';
    else
        return '0';
}

char TraceLine::strategy_k(int left, int right, serial::Serial& ser)
{
    // 如果STIM处于执行状态，直接跳过k策略
    if ( stim == PREDICT_STIM ) {
        return '0';
    }
    if ( stim == N_CHANGE_STIM && state != STRIGHT ) {
        return '0';
    }

    int m   = (left + right) / 2;
    int std = 310;
    if ( left == -INF && right == INF )
        return '0';
    else if ( left == -INF ) {
        cout << "only right line" << endl;
        m = right;
        cout << "m is " << m << endl;

        // 预防圆弧段的误差
        //        if ( m < 0 ) {
        //            return '0';
        //        }
        std = 800;   // 670
        if ( left_sign == 1 ) {
            left_sum--;
            if ( left_sum < 0 ) {
                left_sign = 0;
                left_sum  = 0;
            }
        }
        m = m - std;
        if ( m > 200 ) {
            return '5';
        }
        else if ( m > 50 ) {
            return '4';
        }
        else if ( m > -25 ) {
            return '3';
        }
        else if ( m > -100 ) {
            return '2';
        }
        else {
            return '1';
        }
    }
    else if ( right == INF ) {
        cout << "left sum is " << left_sum << endl;
        int t11 = 13;
        if ( left_sign == 0 && left_sum < t11 ) {
            left_sum++;
            return '0';
        }
        else if ( left_sum == t11 ) {
            left_sign = 1;
        }
        m = left;
        cout << "only left line" << endl;
        std       = -150;   //-130
        Mat img_s = img_resize;
        cout << "m is " << m << endl;

        if ( (m - std) < -250 )
            return '3';
        else if ( (m - std) < -200 )   // 150
            return '4';
        else   // else if ( (m - std) < -100 )   //-50
            return '5';

        // 这是在非转弯情况下的单线策略
        //        else if ( (m - std) < -50 ) {
        //            return '1';
        //        }
        //        else if ( (m - std) < -20 ) {
        //            return '2';
        //        }
        //        else if ( (m - std) < 20 ) {
        //            return '3';
        //        }
        //        else if ( (m - std) < 50 ) {
        //            return '4';
        //        }
        //        else {
        //            return '5';
        //        }
    }
    else {
        cout << "left sum is " << left_sum << endl;
        cout << "two lines" << endl;
        if ( left_sign == 1 ) {
            left_sum--;
            if ( left_sum < 0 ) {
                left_sign = 0;
                left_sum  = 0;
            }
        }
    }
    m = m - std;
    //  if (m < -50)
    //    return '1';
    //  else if (m < 60)
    //    return '2';
    //  else if (m < 145)
    //    return '3';
    //  else if (m < 250)
    //    return '4';
    //  else {
    //    cout << "m :" << m << endl;
    //    return '5';
    //  }
    //  if (m <-50)
    //      return '1';
    //  else if (m <-20)
    //      return '2';
    //  else if (m <75)
    //      return '3';
    //  else if (m < 130)
    //      return '4';
    //  return '5';
    cout << m << endl;

    // 针对路径的规定
    if ( m < -120 ) {
        return '2';
    }
    else if ( m < -70 ) {
        return '3';
    }
    else if ( m < -50 )
        return '1';
    else if ( m < 20 )
        return '2';
    else if ( m < 110 )
        return '3';
    else if ( m < 190 )
        return '4';
    return '5';
}

char TraceLine::strategy_predict(vector<int>& N)
{
    switch ( state ) {
    case STRIGHT:
        // 原来是20
        if ( N[2] < 100 ) {
            return '3';
        }
        else {
            //            int t1 = N[0] + N[1];
            //            int t2 = N[3] + N[4];
            return N[1] > N[3] ? '4' : '2';
        }
    case LEFT:
        if(stim==2)return '3';
            if(stim==0)return '1';
            return '3';
//        return stim == 2 ? '3' : '1';
//        if ( N[1] < 500 ) {   // 100
//        }
//        else {
//            if ( N[2] < 100 ) {
//                return '3';
//            }
//            if ( N[3] < 100 ) {
//                return '4';
//            }
//            if ( N[4] < 100 ) {
//                return '5';
//            }
//            return '2';
//        }
    case RIGHT:
        if ( stim == 0 ) {
            return '5';
        }
        if ( N[4] < 500 ) {
            return '5';
        }
        else {
            if ( N[3] < 100 ) {
                return '4';
            }
            if ( N[2] < 100 ) {
                return '3';
            }
            if ( N[1] < 100 ) {
                return '2';
            }
            return '5';
        }
    }
    return '3';
}

char TraceLine::strategy_short_blue(pair<int, int> p, serial::Serial& ser)
{
    if ( p.first == 0 && p.second == 0 )
        return '0';
    cout << "(" << p.first << "," << p.second << ")" << endl;
    // if(p.first>250  &&p.second >300 &&p.first<400 && p.second <400 )
    if ( p.second > 450 && p.first > 50 ) {
        int stim_blue = 0;
        ros::param::get("stim_blue", stim_blue);
        ros::param::get("stim", stim);
        if ( stim_blue == 1 && stim == 1 ) {
            cout << "first sleep" << endl;
            auto* tx_buf = new uint8_t[9];
            tx_buf[0]    = 'R';
            if ( p.first < 120 )
                tx_buf[1] = '0';
            else
                tx_buf[1] = '1';
            tx_buf[2] = '0';
            tx_buf[3] = '0';
            tx_buf[4] = 'W';
            tx_buf[5] = '4';
            tx_buf[6] = '\r';
            tx_buf[7] = '\n';
            tx_buf[8] = '\0';
            ser.write(tx_buf, 8);
            for ( int i = 0; i < 100; i++ )
                usleep(10000);
            cout << "blue sleep over" << endl;
            ros::param::set("stim_blue", 0);
        }
    }
    return '0';
    // else if(p.first>  &&p.second > &&p.first< && p.second <)
}
void TraceLine::read_src()
{
    // 读入数据
    fstream f("/home/nano/Downloads/Cpp/TEMPLATE/src.txt", ios::in | ios::out);
    for ( int j = 0; j < 5; j++ ) {
        vector<pair<int, int>> temp(477 - 340);
        for ( int i = 340; i < 477; i++ ) {
            f >> temp[i - 340].first;
        }
        for ( int i = 340; i < 477; i++ ) {
            f >> temp[i - 340].second;
        }
        T.push_back(temp);
    }
    f.close();
}

char TraceLine::line_loop(Mat& img_rec, serial::Serial& ser)
{
#if IMG_WRITE
    sprintf(img_path, "/home/nano/Desktop/src/img/img_%d.jpg", cnt);
    imwrite(img_path, img_resize);
    f << "state: " << state << "    stim: " << stim << "    ";
#endif
    // 运行之前，获取state的值，获取stim的值
    // 运行之后，设置state的值，设置stim的值
    // 更新state
    state = q.front();
    resize(img_rec, img_resize, Size(640, 480));
#if IMG_SHOW
    imshow("img_resize", img_resize);
#endif
    char gear = '0';
    // 蓝线策略//////////////////////////////////////////////////////////////////////
    Mat  img_blue  = img_resize(Range(320, 480), Range(0, 640));
    Line temp_blue = calculate_blue(img_blue);
    gear           = strategy_blue(temp_blue);
    if ( gear == '0' ) {
        if ( state == DAOCHE ) {
            return 'd';
        }
        // 短线策略//////////////////////////////////////////////////////////////////
        //  Mat            img_s = img_resize;
        //  pair<int, int> p_t   = calculate_short_blue(img_s);
        // gear                 = strategy_short_blue(p_t,ser);
        // if ( gear == '0' ) {
        // 斜率策略///////////////////////////////////////////////////////////////
        Mat            img_k = img_resize(Range(300, 480), Range(0, 640));
        pair<int, int> lr    = calculate_k(img_k);
        gear                 = strategy_k(lr.first, lr.second, ser);
        if ( gear == '0' ) {
            // 轨迹预测///////////////////////////////////////////////////////////
            vector<int> N = calculate_predict();
            gear          = strategy_predict(N);
            cout << "\033[32mguiji: " << gear << "\033[0m" << endl;
#if IMG_WRITE
            f << "guiji: " << gear << endl;
#endif
        }
        else {
            cout << "k: " << gear << endl;
#if IMG_WRITE
            f << "k: " << gear << endl;
#endif
        }
        // }
        //    else {
        //      cout << "\033[32mblue dot: " << gear << "\033[0m" << endl;
        //      }
    }
    else {
        cout << "\033[32mblue: " << gear << "\033[0m" << endl;
#if IMG_WRITE
        f << "blue: " << gear << "斜率为： " << temp_blue.k << endl;
#endif
    }
    cnt++;
    return gear;
}
